home *** CD-ROM | disk | FTP | other *** search
/ Champak 141 / (Vol 141) Oct 17 2011.iso / Games / despereaux-swings.swf / scripts / Box2D / Dynamics / Joints / b2DistanceJoint.as next >
Encoding:
Text File  |  2011-10-17  |  10.2 KB  |  262 lines
  1. package Box2D.Dynamics.Joints
  2. {
  3.    import Box2D.Common.Math.b2Mat22;
  4.    import Box2D.Common.Math.b2Math;
  5.    import Box2D.Common.Math.b2Vec2;
  6.    import Box2D.Common.b2Settings;
  7.    import Box2D.Dynamics.b2Body;
  8.    import Box2D.Dynamics.b2TimeStep;
  9.    
  10.    public class b2DistanceJoint extends b2Joint
  11.    {
  12.        
  13.       
  14.       public var m_mass:Number;
  15.       
  16.       public var m_u:b2Vec2;
  17.       
  18.       public var m_impulse:Number;
  19.       
  20.       public var m_dampingRatio:Number;
  21.       
  22.       public var m_frequencyHz:Number;
  23.       
  24.       public var m_localAnchor2:b2Vec2;
  25.       
  26.       public var m_localAnchor1:b2Vec2;
  27.       
  28.       public var m_bias:Number;
  29.       
  30.       public var m_gamma:Number;
  31.       
  32.       public var m_length:Number;
  33.       
  34.       public function b2DistanceJoint(param1:b2DistanceJointDef)
  35.       {
  36.          var _loc2_:b2Mat22 = null;
  37.          var _loc3_:Number = NaN;
  38.          var _loc4_:Number = NaN;
  39.          m_localAnchor1 = new b2Vec2();
  40.          m_localAnchor2 = new b2Vec2();
  41.          m_u = new b2Vec2();
  42.          super(param1);
  43.          m_localAnchor1.SetV(param1.localAnchor1);
  44.          m_localAnchor2.SetV(param1.localAnchor2);
  45.          m_length = param1.length;
  46.          m_frequencyHz = param1.frequencyHz;
  47.          m_dampingRatio = param1.dampingRatio;
  48.          m_impulse = 0;
  49.          m_gamma = 0;
  50.          m_bias = 0;
  51.          m_inv_dt = 0;
  52.       }
  53.       
  54.       override public function GetAnchor1() : b2Vec2
  55.       {
  56.          return m_body1.GetWorldPoint(m_localAnchor1);
  57.       }
  58.       
  59.       override public function GetAnchor2() : b2Vec2
  60.       {
  61.          return m_body2.GetWorldPoint(m_localAnchor2);
  62.       }
  63.       
  64.       override public function GetReactionForce() : b2Vec2
  65.       {
  66.          var _loc1_:b2Vec2 = null;
  67.          _loc1_ = new b2Vec2();
  68.          _loc1_.SetV(m_u);
  69.          _loc1_.Multiply(m_inv_dt * m_impulse);
  70.          return _loc1_;
  71.       }
  72.       
  73.       override public function SolvePositionConstraints() : Boolean
  74.       {
  75.          var _loc1_:b2Mat22 = null;
  76.          var _loc2_:b2Body = null;
  77.          var _loc3_:b2Body = null;
  78.          var _loc4_:Number = NaN;
  79.          var _loc5_:Number = NaN;
  80.          var _loc6_:Number = NaN;
  81.          var _loc7_:Number = NaN;
  82.          var _loc8_:Number = NaN;
  83.          var _loc9_:Number = NaN;
  84.          var _loc10_:Number = NaN;
  85.          var _loc11_:Number = NaN;
  86.          var _loc12_:Number = NaN;
  87.          var _loc13_:Number = NaN;
  88.          var _loc14_:Number = NaN;
  89.          var _loc15_:Number = NaN;
  90.          if(m_frequencyHz > 0)
  91.          {
  92.             return true;
  93.          }
  94.          _loc2_ = m_body1;
  95.          _loc3_ = m_body2;
  96.          _loc1_ = _loc2_.m_xf.R;
  97.          _loc4_ = m_localAnchor1.x - _loc2_.m_sweep.localCenter.x;
  98.          _loc5_ = m_localAnchor1.y - _loc2_.m_sweep.localCenter.y;
  99.          _loc6_ = _loc1_.col1.x * _loc4_ + _loc1_.col2.x * _loc5_;
  100.          _loc5_ = _loc1_.col1.y * _loc4_ + _loc1_.col2.y * _loc5_;
  101.          _loc4_ = _loc6_;
  102.          _loc1_ = _loc3_.m_xf.R;
  103.          _loc7_ = m_localAnchor2.x - _loc3_.m_sweep.localCenter.x;
  104.          _loc8_ = m_localAnchor2.y - _loc3_.m_sweep.localCenter.y;
  105.          _loc6_ = _loc1_.col1.x * _loc7_ + _loc1_.col2.x * _loc8_;
  106.          _loc8_ = _loc1_.col1.y * _loc7_ + _loc1_.col2.y * _loc8_;
  107.          _loc7_ = _loc6_;
  108.          _loc9_ = _loc3_.m_sweep.c.x + _loc7_ - _loc2_.m_sweep.c.x - _loc4_;
  109.          _loc10_ = _loc3_.m_sweep.c.y + _loc8_ - _loc2_.m_sweep.c.y - _loc5_;
  110.          _loc11_ = Math.sqrt(_loc9_ * _loc9_ + _loc10_ * _loc10_);
  111.          _loc9_ /= _loc11_;
  112.          _loc10_ /= _loc11_;
  113.          _loc12_ = _loc11_ - m_length;
  114.          _loc12_ = b2Math.b2Clamp(_loc12_,-b2Settings.b2_maxLinearCorrection,b2Settings.b2_maxLinearCorrection);
  115.          _loc13_ = -m_mass * _loc12_;
  116.          m_u.Set(_loc9_,_loc10_);
  117.          _loc14_ = _loc13_ * m_u.x;
  118.          _loc15_ = _loc13_ * m_u.y;
  119.          _loc2_.m_sweep.c.x -= _loc2_.m_invMass * _loc14_;
  120.          _loc2_.m_sweep.c.y -= _loc2_.m_invMass * _loc15_;
  121.          _loc2_.m_sweep.a -= _loc2_.m_invI * (_loc4_ * _loc15_ - _loc5_ * _loc14_);
  122.          _loc3_.m_sweep.c.x += _loc3_.m_invMass * _loc14_;
  123.          _loc3_.m_sweep.c.y += _loc3_.m_invMass * _loc15_;
  124.          _loc3_.m_sweep.a += _loc3_.m_invI * (_loc7_ * _loc15_ - _loc8_ * _loc14_);
  125.          _loc2_.SynchronizeTransform();
  126.          _loc3_.SynchronizeTransform();
  127.          return b2Math.b2Abs(_loc12_) < b2Settings.b2_linearSlop;
  128.       }
  129.       
  130.       override public function InitVelocityConstraints(param1:b2TimeStep) : void
  131.       {
  132.          var _loc2_:b2Mat22 = null;
  133.          var _loc3_:Number = NaN;
  134.          var _loc4_:b2Body = null;
  135.          var _loc5_:b2Body = null;
  136.          var _loc6_:Number = NaN;
  137.          var _loc7_:Number = NaN;
  138.          var _loc8_:Number = NaN;
  139.          var _loc9_:Number = NaN;
  140.          var _loc10_:Number = NaN;
  141.          var _loc11_:Number = NaN;
  142.          var _loc12_:Number = NaN;
  143.          var _loc13_:Number = NaN;
  144.          var _loc14_:Number = NaN;
  145.          var _loc15_:Number = NaN;
  146.          var _loc16_:Number = NaN;
  147.          var _loc17_:Number = NaN;
  148.          var _loc18_:Number = NaN;
  149.          var _loc19_:Number = NaN;
  150.          m_inv_dt = param1.inv_dt;
  151.          _loc4_ = m_body1;
  152.          _loc5_ = m_body2;
  153.          _loc2_ = _loc4_.m_xf.R;
  154.          _loc6_ = m_localAnchor1.x - _loc4_.m_sweep.localCenter.x;
  155.          _loc7_ = m_localAnchor1.y - _loc4_.m_sweep.localCenter.y;
  156.          _loc3_ = _loc2_.col1.x * _loc6_ + _loc2_.col2.x * _loc7_;
  157.          _loc7_ = _loc2_.col1.y * _loc6_ + _loc2_.col2.y * _loc7_;
  158.          _loc6_ = _loc3_;
  159.          _loc2_ = _loc5_.m_xf.R;
  160.          _loc8_ = m_localAnchor2.x - _loc5_.m_sweep.localCenter.x;
  161.          _loc9_ = m_localAnchor2.y - _loc5_.m_sweep.localCenter.y;
  162.          _loc3_ = _loc2_.col1.x * _loc8_ + _loc2_.col2.x * _loc9_;
  163.          _loc9_ = _loc2_.col1.y * _loc8_ + _loc2_.col2.y * _loc9_;
  164.          _loc8_ = _loc3_;
  165.          m_u.x = _loc5_.m_sweep.c.x + _loc8_ - _loc4_.m_sweep.c.x - _loc6_;
  166.          m_u.y = _loc5_.m_sweep.c.y + _loc9_ - _loc4_.m_sweep.c.y - _loc7_;
  167.          if((_loc10_ = Math.sqrt(m_u.x * m_u.x + m_u.y * m_u.y)) > b2Settings.b2_linearSlop)
  168.          {
  169.             m_u.Multiply(1 / _loc10_);
  170.          }
  171.          else
  172.          {
  173.             m_u.SetZero();
  174.          }
  175.          _loc11_ = _loc6_ * m_u.y - _loc7_ * m_u.x;
  176.          _loc12_ = _loc8_ * m_u.y - _loc9_ * m_u.x;
  177.          _loc13_ = _loc4_.m_invMass + _loc4_.m_invI * _loc11_ * _loc11_ + _loc5_.m_invMass + _loc5_.m_invI * _loc12_ * _loc12_;
  178.          m_mass = 1 / _loc13_;
  179.          if(m_frequencyHz > 0)
  180.          {
  181.             _loc14_ = _loc10_ - m_length;
  182.             _loc15_ = 2 * Math.PI * m_frequencyHz;
  183.             _loc16_ = 2 * m_mass * m_dampingRatio * _loc15_;
  184.             _loc17_ = m_mass * _loc15_ * _loc15_;
  185.             m_gamma = 1 / (param1.dt * (_loc16_ + param1.dt * _loc17_));
  186.             m_bias = _loc14_ * param1.dt * _loc17_ * m_gamma;
  187.             m_mass = 1 / (_loc13_ + m_gamma);
  188.          }
  189.          if(param1.warmStarting)
  190.          {
  191.             m_impulse *= param1.dtRatio;
  192.             _loc18_ = m_impulse * m_u.x;
  193.             _loc19_ = m_impulse * m_u.y;
  194.             _loc4_.m_linearVelocity.x -= _loc4_.m_invMass * _loc18_;
  195.             _loc4_.m_linearVelocity.y -= _loc4_.m_invMass * _loc19_;
  196.             _loc4_.m_angularVelocity -= _loc4_.m_invI * (_loc6_ * _loc19_ - _loc7_ * _loc18_);
  197.             _loc5_.m_linearVelocity.x += _loc5_.m_invMass * _loc18_;
  198.             _loc5_.m_linearVelocity.y += _loc5_.m_invMass * _loc19_;
  199.             _loc5_.m_angularVelocity += _loc5_.m_invI * (_loc8_ * _loc19_ - _loc9_ * _loc18_);
  200.          }
  201.          else
  202.          {
  203.             m_impulse = 0;
  204.          }
  205.       }
  206.       
  207.       override public function GetReactionTorque() : Number
  208.       {
  209.          return 0;
  210.       }
  211.       
  212.       override public function SolveVelocityConstraints(param1:b2TimeStep) : void
  213.       {
  214.          var _loc2_:b2Mat22 = null;
  215.          var _loc3_:b2Body = null;
  216.          var _loc4_:b2Body = null;
  217.          var _loc5_:Number = NaN;
  218.          var _loc6_:Number = NaN;
  219.          var _loc7_:Number = NaN;
  220.          var _loc8_:Number = NaN;
  221.          var _loc9_:Number = NaN;
  222.          var _loc10_:Number = NaN;
  223.          var _loc11_:Number = NaN;
  224.          var _loc12_:Number = NaN;
  225.          var _loc13_:Number = NaN;
  226.          var _loc14_:Number = NaN;
  227.          var _loc15_:Number = NaN;
  228.          var _loc16_:Number = NaN;
  229.          var _loc17_:Number = NaN;
  230.          _loc3_ = m_body1;
  231.          _loc4_ = m_body2;
  232.          _loc2_ = _loc3_.m_xf.R;
  233.          _loc5_ = m_localAnchor1.x - _loc3_.m_sweep.localCenter.x;
  234.          _loc6_ = m_localAnchor1.y - _loc3_.m_sweep.localCenter.y;
  235.          _loc7_ = _loc2_.col1.x * _loc5_ + _loc2_.col2.x * _loc6_;
  236.          _loc6_ = _loc2_.col1.y * _loc5_ + _loc2_.col2.y * _loc6_;
  237.          _loc5_ = _loc7_;
  238.          _loc2_ = _loc4_.m_xf.R;
  239.          _loc8_ = m_localAnchor2.x - _loc4_.m_sweep.localCenter.x;
  240.          _loc9_ = m_localAnchor2.y - _loc4_.m_sweep.localCenter.y;
  241.          _loc7_ = _loc2_.col1.x * _loc8_ + _loc2_.col2.x * _loc9_;
  242.          _loc9_ = _loc2_.col1.y * _loc8_ + _loc2_.col2.y * _loc9_;
  243.          _loc8_ = _loc7_;
  244.          _loc10_ = _loc3_.m_linearVelocity.x + -_loc3_.m_angularVelocity * _loc6_;
  245.          _loc11_ = _loc3_.m_linearVelocity.y + _loc3_.m_angularVelocity * _loc5_;
  246.          _loc12_ = _loc4_.m_linearVelocity.x + -_loc4_.m_angularVelocity * _loc9_;
  247.          _loc13_ = _loc4_.m_linearVelocity.y + _loc4_.m_angularVelocity * _loc8_;
  248.          _loc14_ = m_u.x * (_loc12_ - _loc10_) + m_u.y * (_loc13_ - _loc11_);
  249.          _loc15_ = -m_mass * (_loc14_ + m_bias + m_gamma * m_impulse);
  250.          m_impulse += _loc15_;
  251.          _loc16_ = _loc15_ * m_u.x;
  252.          _loc17_ = _loc15_ * m_u.y;
  253.          _loc3_.m_linearVelocity.x -= _loc3_.m_invMass * _loc16_;
  254.          _loc3_.m_linearVelocity.y -= _loc3_.m_invMass * _loc17_;
  255.          _loc3_.m_angularVelocity -= _loc3_.m_invI * (_loc5_ * _loc17_ - _loc6_ * _loc16_);
  256.          _loc4_.m_linearVelocity.x += _loc4_.m_invMass * _loc16_;
  257.          _loc4_.m_linearVelocity.y += _loc4_.m_invMass * _loc17_;
  258.          _loc4_.m_angularVelocity += _loc4_.m_invI * (_loc8_ * _loc17_ - _loc9_ * _loc16_);
  259.       }
  260.    }
  261. }
  262.